/*
 * controller.cpp
 *
 *      Author: Jerome RD Soine and Christoph A Brand
 *      Institution: Schwarz goup, Institute for Theoretical Physics, Heidelberg University, Germany
 */

#include "../../../../include/base/cell_models/ACM/ActiveCableModel.h"
#include "../../../../include/base/cell_models/ACM/numerics/minimizer.h"
#include "../../../../include/base/cell_models/ACM/geometry/util/vtk_color/elasticenergyextractor.h"
#include "../../../../include/base/cell_models/ACM/geometry/util/vtk_color/elasticforceextractor.h"
#include "../../../../include/base/cell_models/ACM/geometry/util/vtk_color/fibretypeextractor.h"
#include "../../../../include/base/cell_models/ACM/geometry/util/vtk_color/fixedpointsextractor.h"
#include "../../../../include/base/cell_models/ACM/geometry/base/edge.h"
#include "../../../../include/base/cell_models/ACM/energy/include_energies.h"
#include "math.h"
#include "../../../../include/core/utility/VectorOperations.h"
#include "../../../../include/core/shape/Circle.h"
#include <cmath>


#include <cstdlib>
#include <iostream>
#include <cmath>

ActiveCableModel::ActiveCableModel(string filename, string output_folder, double spring_constant, vector<double> param){
	//cout << "Generating controller" << endl;
	snapshot_interval = -1;
	FILENAME = filename;
	OUTPUT_FOLDER = output_folder;
	SPRING_CONSTANT = spring_constant;
	parameters = param;

	g = new GeomObject(filename, output_folder,Vertex(0,0,10),false);
	min = new Minimizer(g,Minimizer::MODE_CONJUGATE_GRADIENTS);
	//cout << "# vertices: " << g->getVertices()->size() << endl;

	//cout << "Initializing stress fibres" << endl;
	initFibres(parameters);

	//cout << "Initializing active cable energy" << endl;
	ActiveCableEnergy * e = new ActiveCableEnergy(g, spring_constant);
	min->getEnergies()->push_back(e);

	//cout << "Initializing graphical output generators" << endl;
	ElasticForceExtractor * efe = new ElasticForceExtractor(e);
	ElasticEnergyExtractor * eee = new ElasticEnergyExtractor(e,efe);
	FibreTypeExtractor * fte = new FibreTypeExtractor(g,eee);
	g->setColorDataExtractor(fte);
	g->setFixedPointsExtractor(new FixedPointsExtractor(g));

	fiber_list = getFiberList(filename);
}

ActiveCableModel * ActiveCableModel::clone() const{
	return new ActiveCableModel(FILENAME, OUTPUT_FOLDER, SPRING_CONSTANT, parameters);
}

void ActiveCableModel::setTensions(vector<double> tensions){
	initFibres(tensions);
}


void ActiveCableModel::initFibres(vector<double> tensions){
//	cout << "\t ACM:initFibers\n";
	vector<Edge*> edges = *g->getEdges();
	int max_nr = 0;
	for(unsigned int i=0; i<edges.size(); i++){
		int type = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRETYPE));
		if(type == TYPE_NETWORK){
			edges[i]->putNumericalAttribute(FIELD_TENSION, tensions[0]);
			//cout << "\t\t ACM:initFibers: network link created\n";
		} else {
			int nr = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRENUMBER));
			edges[i]->putNumericalAttribute(FIELD_TENSION, tensions[nr]);
			if(nr>max_nr) max_nr = nr;
			//cout << "\t\t ACM:initFibers: fiber link created\n";
		}
	}

//	for(unsigned int i=0; i<edges.size(); i++){
//		int nr = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRENUMBER));
//		double tension = (int) round(edges[i]->getValueOfAttribute(FIELD_TENSION));
//		if(tension != 0) cout << "Nonzero tension in fiber " << nr << ": " << tension << endl;
//	}
	//Check correct read in
//	VectorOperations::cout_std_vector(tensions);
	for(int i=0; i<max_nr; i++){
		//cout << tensions[i] << endl;
		if(std::isnan(tensions[i])) {
			cout << "Fatal error: Tension #" << i << " is NaN." << endl;
			exit(1);
		}
	}
}

vector<int> ActiveCableModel::getFiberList(string filename){
	GeomObject * g = new GeomObject(filename, "", Vertex(1,1,10),false);
	vector<int> types, numbers;
	vector<Edge*> edges = *g->getEdges();
	for(unsigned int i=0; i<edges.size(); i++){
		int type = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRETYPE));
		int number = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRENUMBER));
		if(type != TYPE_NETWORK){
			//cout << type << " - " << number << endl;
			bool alreadyFound = false;
			if(numbers.size() > 0){
				for(unsigned int i=0; i<numbers.size();i++){
					if(numbers[i] == number) alreadyFound = true;
				}
			}
			if(!alreadyFound){
				numbers.push_back(number);
				types.push_back(type);
			}
		}
	}

	vector<int> ret(types.size(),0);
	for(unsigned int i=0; i<numbers.size();i++){
		ret[numbers[i]-1] = types[i];
	}
	delete g;
	//for(unsigned int i=0; i<numbers.size();i++) cout << ret[i] << endl;
	return ret;
}

vector<ForceTransmission*> ActiveCableModel::getForceTransmissionShapes(){
	vector<ForceTransmission*> fts;
	vector<Vertex*> vertices = * g->getVertices();
	for(unsigned int i=0; i<vertices.size();i++){
		if(vertices[i]->isFixed()){
			double area = 150;
			vector<double> f(2,0);
			vector<double> c(2,0);
			c[0] = vertices[i]->getX();
			c[1] = vertices[i]->getY();
			ForceTransmission * ft = new ForceTransmission(f, new Circle(c, sqrt(area/3.14159265358979323846)));
			fts.push_back(ft);
		}
	}
	return fts;
}

void ActiveCableModel::run(){
	//Minimize and specify force transmissions
	minimize(100000);
	for(unsigned int i=0; i<force_transmissions.size(); i++) delete force_transmissions[i];
	vector<ForceTransmission*> fts;
	vector<Vertex*> vertices = * g->getVertices();
	for(unsigned int i=0; i<vertices.size();i++){
		if(vertices[i]->isFixed()){
			Vertex force = (min->getTotalGradientOfFixedPoints(*vertices[i]))*(-1);
			vector<double> f(2,0);
			f[0] = force.getX();
			f[1] = force.getY();
			vector<double> c(2,0);
			c[0] = vertices[i]->getX();
			c[1] = vertices[i]->getY();
			double area = 150;
			ForceTransmission * ft = new ForceTransmission(f, new Circle(c, sqrt(area/3.14159265358979323846)));
			fts.push_back(ft);
		}
	}
	force_transmissions = fts;
}


void ActiveCableModel::minimize(int max_iterations){
	int iteration = 0;

	try{
		do {
			if(snapshot_interval > 0) {
				if(iteration%snapshot_interval==0) g->saveSnapshot();
			}
			++iteration;
			min->iterate();
		} while (iteration <= max_iterations);

	} catch (int MinimizerExceptionCode){
		if(MinimizerExceptionCode == Minimizer::ERROR_BRACKETING_MINIMUM){
			cout << "Minimum could not be bracketed - Assuming numerical instability due to small gradients. Network is therefore fully equilibrated.\n";
		} else if (MinimizerExceptionCode == Minimizer::ERROR_TOO_MANY_ITERATIONS_IN_GOLDEN_SECTION_SEARCH){
			//cout << "Could not finish golden section search - Assuming zero tension.\n";
		} else {
			cout << "Unresolved error. Continuing.\n";
		}
	}
	cout << "Network equilibration took " << iteration << " iterations\n";
}

vector<Vertex> ActiveCableModel::getTractions(){
	vector<Vertex*> vertices = * g->getVertices();
	vector<Vertex> tractions;
	tractions.reserve(vertices.size());
	for(unsigned int i=0; i<vertices.size();i++){
		double area = 1;//vertices[i]->getAssociatedArea();
		Vertex force = (min->getTotalGradientOfFixedPoints(*vertices[i]))*(-1);
		tractions.push_back(force/area);
	}
	return tractions;
}

vector<double> ActiveCableModel::get_final_configuration(){
	vector<Vertex*> vertices = * g->getVertices();
	vector<double> copy;
	copy.reserve(vertices.size()*2);
	for(unsigned int i=0; i<vertices.size();i++){
		copy.push_back(vertices[i]->getX());
		copy.push_back(vertices[i]->getY());
	}
	return copy;
}

vector<Vertex> ActiveCableModel::copyVertices(){
	vector<Vertex*> vertices = * g->getVertices();
	vector<Vertex> copy;
	copy.reserve(vertices.size());
	for(unsigned int i=0; i<vertices.size();i++){
		copy.push_back((*vertices[i]));
	}
	return copy;
}

void ActiveCableModel::set_initial_configuration(vector<double> config){
	vector<Vertex> pos;
	pos.reserve(config.size()/2);
	for(unsigned int i=0; i<config.size()/2; i++){
		Vertex p(config[2*i], config[2*i+1], 0);
		pos.push_back(p);
	}
	setInitialConfiguration(pos);
}

void ActiveCableModel::setInitialConfiguration(vector<Vertex> pos){
	min->init();
	vector<Vertex*> vertices = * g->getVertices();
	for(unsigned int i=0; i<vertices.size();i++){
		vertices[i]->setX(pos[i].getX());
		vertices[i]->setY(pos[i].getY());
		vertices[i]->setZ(pos[i].getZ());
	}
}

GeomObject* ActiveCableModel::getGeomObject()
{
	return g;
}

vector<int> ActiveCableModel::get_fiber_types()
{
	vector<Edge*> edges = *g->getEdges();
	vector<int> type_list(fiber_list.size(),0);
	for(unsigned int i=0; i<edges.size(); i++){
		int type = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRETYPE));
		int nr = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRENUMBER));
		type_list[nr-1]=type;
	}
	return type_list;
}


vector<double> ActiveCableModel::get_fiber_lengths(){
	vector<Edge*> edges = *g->getEdges();
	vector<double> lengths(fiber_list.size(),0);

	for(unsigned int i=0; i<edges.size(); i++){
		int type = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRETYPE));
		if(type != TYPE_NETWORK){
			int nr = (int) round(edges[i]->getValueOfAttribute(FIELD_FIBRENUMBER));
			// Numbers begin with 1
			lengths[nr-1] += edges[i]->toVector().abs();
		}
	}
	return lengths;
}


void ActiveCableModel::createShiftedVTK(string orig_vtk_filename, string dest_vtk_filename, FieldInterpolation2D * shiftObject){
	GeomObject g(orig_vtk_filename,"",Vertex(0,0,10),false);
	g.setFixedPointsExtractor(new FixedPointsExtractor(&g));
	g.setColorDataExtractor(new FibreTypeExtractor(&g));
	vector<Vertex*> vertices = *g.getVertices();
	for(unsigned int i=0; i<vertices.size(); i++){
		//cout << "\n\nNEW SHIFT\n\n\n";
		Vertex * tomove = vertices[i];
		Vector<double> tmp (2);
		Point<2> p(tomove->getX(), tomove->getY());
		shiftObject->point_evaluation(p, tmp);
		for(int j=0; j<10;j++){
			//if(j==0) cout << "Shift: " << tmp[0] << "," << tmp[1] << endl;
			tomove->setX(tomove->getX()-tmp[0]);
			tomove->setY(tomove->getY()-tmp[1]);
			Vector<double> backShift(2);
			Point<2> controlP(tomove->getX(), tomove->getY());
			shiftObject->point_evaluation(controlP, backShift);
			controlP[0] += backShift[0];
			controlP[1] += backShift[1];

			Point<2> distance = controlP-p;
			tmp[0] = distance[0];
			tmp[1] = distance[1];
		}
	}
	g.saveVTK(dest_vtk_filename);
}

void ActiveCableModel::cut(Edge e){
	vector<Edge*> edges = *g->getEdges();
	vector<Edge*> tmp_edges;
	for(unsigned int i=0;i<edges.size(); i++){
		if(!edges[i]->intersects(e)) {
			tmp_edges.push_back(edges[i]);
		} else {
			edges[i]->getHead()->removeAssociatedEdge(edges[i]);
			edges[i]->getTail()->removeAssociatedEdge(edges[i]);
		}
	}
	g->setEdges(tmp_edges);
}
